/* 美中机器人JOYSITCK_DRIVER，负责解析/joy，发送到/cmd_vel */

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>
#include "boost/thread/mutex.hpp"
#include "boost/thread/thread.hpp"
#include "ros/console.h"

class MZ_JoyDriver
{
public:
    MZ_JoyDriver();

private:
    enum{ LOOP_RATE=10, };  // const value

    void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
    void Publish();

    ros::NodeHandle nh_;

    int linear_x, linear_y, angular_positive, angular_negative;
    double l_scale_, a_scale_;
    bool watch_dog_;
    ros::Publisher vel_pub_;
    ros::Subscriber joy_sub_;

    geometry_msgs::Twist vel_to_pub;
    boost::mutex publish_mutex_;
    ros::Timer timer_;
};

MZ_JoyDriver::MZ_JoyDriver():
    linear_x(1),
    linear_y(0),
    angular_positive(4),
    angular_negative(5)
{
    ros::param::param<double>("~scale_linear", l_scale_, 0.2);
    ros::param::param<double>("~scale_angular", a_scale_, 0.5);
    vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1, true);
    joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &MZ_JoyDriver::joyCallback, this);

    timer_ = nh_.createTimer(ros::Duration(1.0/LOOP_RATE), boost::bind(&MZ_JoyDriver::Publish, this));
}

void MZ_JoyDriver::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{ 
    vel_to_pub.angular.z = a_scale_*(joy->buttons[angular_positive]-joy->buttons[angular_negative]);
    vel_to_pub.linear.x = l_scale_ * joy->axes[linear_x];
    vel_to_pub.linear.y = l_scale_ * joy->axes[linear_y];
    watch_dog_ = 1;  // 喂狗
}

void MZ_JoyDriver::Publish()
{
    static int counts = 0;
    boost::mutex::scoped_lock lock(publish_mutex_);
    if(watch_dog_)
        vel_pub_.publish(vel_to_pub);
    else {  // 一个喂狗周期内未收到joy消息，则向cmd_vel发0
        geometry_msgs::Twist foo;
        vel_pub_.publish(foo);
    }
    counts++;
    if(counts == LOOP_RATE){  // 每秒狗饿一次
        watch_dog_ = 0;
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "MZ_joy_driver");
    MZ_JoyDriver my_joy_driver;
    ros::spin();
}
